#ifndef QUATERNION_MANIFOLD_H
#define QUATERNION_MANIFOLD_H

#include <ceres/manifold.h>
#include <Eigen/Dense>

class QuaternionManifold : public ceres::Manifold {
// public:
//     int AmbientSize() const override { return 4; }
//     int TangentSize() const override { return 3; }

//     bool Plus(const double* x,
//               const double* delta,
//               double* x_plus_delta) const override {
//         const double norm_delta = sqrt(delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2]);
    
//         if (norm_delta < 1e-10) {
//             for (int i = 0; i < 4; ++i) {
//                 x_plus_delta[i] = x[i];
//             }
//             return true;
//         }
    
//         const double half_norm = norm_delta * 0.5;
//         const double sin_half = sin(half_norm);
//         const double cos_half = cos(half_norm);
//         // 使用指数映射：exp(δ) = [sin(||δ||/2)*δ/||δ||, cos(||δ||/2)]
//         double q_delta[4];
//         const double scale = sin_half / norm_delta;
//         q_delta[0] = scale * delta[0];
//         q_delta[1] = scale * delta[1];
//         q_delta[2] = scale * delta[2];
//         q_delta[3] = cos_half;
    
//         // 四元数乘法 (正确顺序)， 然后进行四元数乘法：x_plus_delta = exp(δ) ⊗ x
//         x_plus_delta[0] = q_delta[3]*x[0] + q_delta[0]*x[3] + q_delta[1]*x[2] - q_delta[2]*x[1];
//         x_plus_delta[1] = q_delta[3]*x[1] - q_delta[0]*x[2] + q_delta[1]*x[3] + q_delta[2]*x[0];
//         x_plus_delta[2] = q_delta[3]*x[2] + q_delta[0]*x[1] - q_delta[1]*x[0] + q_delta[2]*x[3];
//         x_plus_delta[3] = q_delta[3]*x[3] - q_delta[0]*x[0] - q_delta[1]*x[1] - q_delta[2]*x[2];
    
//         // 归一化
//         const double norm = sqrt(x_plus_delta[0]*x_plus_delta[0] +
//                            x_plus_delta[1]*x_plus_delta[1] +
//                            x_plus_delta[2]*x_plus_delta[2] +
//                            x_plus_delta[3]*x_plus_delta[3]);
    
//         for (int i = 0; i < 4; ++i) {
//             x_plus_delta[i] /= norm;
//         }
//         return true;
//     }

//     bool PlusJacobian(const double* x, double* jacobian) const override {
//         // 雅可比矩阵是 [0.5*I; 0] 的形式
//         Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> J(jacobian);
//         J.setZero();
//         J(0, 0) = 0.5;
//         J(1, 1) = 0.5;
//         J(2, 2) = 0.5;
//         return true;
//     }

//     bool Minus(const double* y,
//                const double* x,
//                double* y_minus_x) const override {
//         // x^{-1} * y
//         double x_inv[4] = {-x[0], -x[1], -x[2], x[3]};
//         double q_delta[4];
        
//         q_delta[0] = x_inv[3] * y[0] + x_inv[2] * y[1] - x_inv[1] * y[2] + x_inv[0] * y[3];
//         q_delta[1] = -x_inv[2] * y[0] + x_inv[3] * y[1] + x_inv[0] * y[2] + x_inv[1] * y[3];
//         q_delta[2] = x_inv[1] * y[0] - x_inv[0] * y[1] + x_inv[3] * y[2] + x_inv[2] * y[3];
//         q_delta[3] = -x_inv[0] * y[0] - x_inv[1] * y[1] - x_inv[2] * y[2] + x_inv[3] * y[3];
        
//         // 确保标量部分为正
//         if (q_delta[3] < 0.0) {
//             for (int i = 0; i < 4; ++i) {
//                 q_delta[i] = -q_delta[i];
//             }
//         }
        
//         const double sin_theta = sqrt(q_delta[0] * q_delta[0] +
//                                      q_delta[1] * q_delta[1] +
//                                      q_delta[2] * q_delta[2]);
        
//         if (sin_theta < 1e-10) {
//             y_minus_x[0] = 0.0;
//             y_minus_x[1] = 0.0;
//             y_minus_x[2] = 0.0;
//             return true;
//         }
        
//         const double theta = 2.0 * atan2(sin_theta, q_delta[3]);
//         const double scale = theta / sin_theta;
        
//         y_minus_x[0] = scale * q_delta[0];
//         y_minus_x[1] = scale * q_delta[1];
//         y_minus_x[2] = scale * q_delta[2];
        
//         return true;
//     }

//     bool MinusJacobian(const double* x, double* jacobian) const override {
//         Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> J(jacobian);
//         J.setZero();
//         J(0, 0) = 2.0;
//         J(1, 1) = 2.0;
//         J(2, 2) = 2.0;
//         return true;
//     }
};

#endif // QUATERNION_MANIFOLD_H